#include "handle.h"

extern uint8_t flag_OnStage;
extern int team;
unsigned long beginTime = 0;
unsigned long CDTime = 0;

char result = ' ';
void outBack()
{
    uint8_t pose = isOnStage();

    switch (pose)
    {

    case (fl_out + fr_out):
        Backward(MIDSPEED);
        delay(200);
        if (direction == LAST_L)
            turnRound_R(EXHIGHSPEED);
        else if (direction == LAST_R)
            turnRound_L(EXHIGHSPEED);
        delay(300);
        break;
    //后方出界
    case (bl_out + br_out):
        Forward(HIGHSPEED);
        delay(500);
        break;
    //左侧出界
    case (fl_out + bl_out):
        turnRound_R(EXHIGHSPEED);
        delay(200);
        break;
    //右侧出界
    case (fr_out + br_out):
        turnRound_L(EXHIGHSPEED);
        delay(200);
        break;
    //左上角出界
    case (fl_out + fr_out + bl_out):
        Backward(MIDSPEED);
        delay(200);
        turnRound_R(EXHIGHSPEED);
        delay(200);
        break;
    //右上角出界
    case (fl_out + fr_out + br_out):
        Backward(MIDSPEED);
        delay(200);
        turnRound_L(EXHIGHSPEED);
        delay(200);
        break;
    //左下角出界
    case (fl_out + bl_out + br_out):
        Forward(HIGHSPEED);
        delay(500);
        break;
    //右下角出界
    case (fr_out + bl_out + br_out):
        Forward(HIGHSPEED);
        delay(500);
        break;
    default:
        break;
    }
}

void objAttack()
{
    uint8_t direction = isObject();
    unsigned long lasttime = 0;
    unsigned long pasttime = 0;

    //设置各个方向上物体处理的优先级
    if (direction & f_object)
    {
        direction = f_object;
    }
    else if (direction & l_object)
    {
        direction = l_object;
    }
    else if (direction & r_object)
    {
        direction = r_object;
    }

    switch (direction)
    {
    case f_object:
        lockObj(); //锁定目标 防止受到其他方向物体的干扰
        break;

    case l_object:
        turnRound_L(MIDSPEED);
        lasttime = millis();
        while (pasttime <= 1000 && isOnStage)
        {

            if (!digitalRead(DOWN_F_PSW))
            {
                delay(100);
                break;
            }
            isOnStage();
            pasttime = millis() - lasttime;
        }

        isObject();

        break;
    case r_object:
        turnRound_R(MIDSPEED);
        lasttime = millis();
        while (pasttime <= 1000 && isOnStage)
        {

            if (!digitalRead(DOWN_F_PSW))
            {
                delay(100);
                break;
            }
            isOnStage();
            pasttime = millis() - lasttime;
        }

        isObject();

        break;
    default:
        flag_object = 0;
        break;
    }
}

void lockObj()
{
    unsigned long lasttime = 0;
    unsigned long pasttime = 0;

    if (digitalRead(DOWN_F_PSW))
    {
        return;
    }
    while (Serial1.available())
    {
        Serial1.read();
    }

    Serial1.println('o'); //发送识别信号
    Serial2.println('o');

    brack();
    delay(300);

    Serial2.println("等待");
    lasttime = millis();
    while (!Serial1.available() && pasttime < 2000)
    {
        pasttime = millis() - lasttime;
        result = '5';
    }

    if (pasttime >= 2000 && !Serial1.available())
    {
        Serial2.println("超时");
    }
    else
    {
        Serial2.print("接收到信号:");
        result = Serial1.read();
        Serial2.println(result);
    }

    if ((result == '0' || result == '2') && (team == RED_TEAM))
    {
        Serial2.println("RED_TEAM");
        while (!digitalRead(DOWN_F_PSW))
        {
            Forward(EXHIGHSPEED);
            isOnStage();
            if (!flag_OnStage)
            {
                brack();
                ServoRun(LIFT);
                delay(200);
                ServoRun(DOWN);
                return;
            }
        }
    }
    else if ((result == '1' || result == '2') && (team == BLUE_TEAM))
    {
        Serial2.println("BLUE_TEAM");
        while (!digitalRead(DOWN_F_PSW))
        {
            Forward(EXHIGHSPEED);
            isOnStage();
            if (!flag_OnStage)
            {
                brack();
                ServoRun(LIFT);
                delay(200);
                ServoRun(DOWN);
                return;
            }
        }
    }
    else if (result == '0' || result == '1')
    {
        Serial2.println("0|1");
        //    beginTime = millis();
        awayObj();
    }

    else if (result == '4' || result == '5') //超时处理
    {
        if (result == '4')
            Serial2.println("识别结构为4");
        else if (result == '5')
            Serial2.println("超时处理");

        awayObj();
    }
}

void awayObj()
{
    unsigned long lasttime = millis();
    unsigned long pasttime = 0;
    uint8_t flag_LR_Stop = 0;
    void (*DirTurn)(Speed PWM);

    Serial2.println("awayObj");

    if (digitalRead(DOWN_F_PSW))
    {
        return;
    }

    if (direction == LAST_L)
        DirTurn = turnRound_R;
    else if (direction == LAST_R)
        DirTurn = turnRound_L;

    while (pasttime < 1500 && flag_OnStage)
    {

        DirTurn(MIDSPEED);

        if (!digitalRead(DOWN_L_PSW) || !digitalRead(DOWN_R_PSW))
        {
            flag_LR_Stop = 1;
            break;
        }

        pasttime = millis() - lasttime;
        isOnStage();
    }

    lasttime = millis();
    pasttime = 0;
    while (pasttime < 1000 && flag_OnStage)
    {

        Forward(MIDSPEED);
        if (flag_LR_Stop)
        {
            if (digitalRead(DOWN_L_PSW) && digitalRead(DOWN_R_PSW))
                return;
        }
        pasttime = millis() - lasttime;
        isOnStage();
    }
}

void fallBack()
{
    uint8_t (*dirTurn)(Speed PWM);
    uint8_t pose = isUnderStage();
    unsigned long lasttime = 0;
    unsigned long pasttime = 0;

    flag_backStage = 0;
  
    if(direction == LAST_L)
    dirTurn = turnRound_L;
    else if(direction == LAST_R)
    dirTurn = turnRound_R;

    switch (pose)
    {
    case 1:             //后方朝向擂台
        Backward(MIDSPEED);
        delay(500);
        brack();
        ServoRun(LIFT);
        delay(500);
        getUpStage();
        delay(500);

        flag_backStage = 1;
        
        break;
    case 2:             //左侧朝向擂台
        turnRound_R(HIGHSPEED + 0);
        delay(100);        
        break;
    case 3:             //右侧朝向擂台
        turnRound_L(HIGHSPEED + 0);
        delay(100);        
        break;    
    case 4:             //前方朝向擂台
        dirTurn(HIGHSPEED + 0);
        delay(100);
        break;
    case 5:             //前左检测到围栏
        turnRound_R(HIGHSPEED + 0);
        delay(100);
        break;
    case 6:             //前右检测到围栏
        turnRound_L(HIGHSPEED + 0);
        delay(100);
        break;
    case 7:             //后左检测到围栏
        Forward(HIGHSPEED + 0);
        delay(100);
        break;
    case 8:             //后右检测到围栏
        Forward(HIGHSPEED + 0);
        delay(100);
        break;
    case 9:             //前方或后方有台上敌人
        dirTurn(HIGHSPEED + 0);
        delay(100);
        break;
    case 10:            //左方或右方有台上敌人
        Forward(HIGHSPEED + 0);
        delay(100);
        break;
    case 11:            //前方、左侧和右侧检测到围栏
        Backward(HIGHSPEED + 0);
        delay(100);
        break;
    case 12:            //前方、右侧和后方检测到围栏
        turnRound_L(HIGHSPEED + 0);
        delay(100);     
        break;
    case 13:            //前方、左侧和后方检测到围栏
        turnRound_R(HIGHSPEED + 0);
        delay(100);
        break;
    case 14:            //右侧、左侧和后方检测到围栏
        Forward(HIGHSPEED + 0);
        delay(100);
        break;
    case 15:            //前方和右侧对擂台，其他传感器没检测到
        turnRound_L(HIGHSPEED + 0);
        delay(100);
        break;
    case 16:            //前方和左侧对擂台，其他传感器没检测到
        turnRound_R(HIGHSPEED + 0);
        delay(100);
        break;
    case 17:            //后方和右侧对擂台，其他传感器没检测到
        Backward(HIGHSPEED + 0);
        delay(100);
        break;
    case 18:            //后方和左侧对擂台，其他传感器没检测到
        Backward(HIGHSPEED + 0);
        delay(100);
        break;
    default:
        int lasttime = millis();
        int pasttime = 0;

        dirTurn(HIGHSPEED);
        while(pasttime < 1000 && isUnderStage() == NONE_toStage)
        {
          pasttime = millis() - lasttime;
        }
        if(pasttime < 1000) return;
        else
        {
            Backward(HIGHSPEED + 0);
            delay(1000);
        }
        break;
    }
}

void getUpStage()
{
    Backward(EXHIGHSPEED);
    delay(300);
    ServoRun(CLIMB);
    delay(700);
    ServoRun(DOWN);
}
